#include "geometry_msgs/msg/transform_stamped.hpp"
#include "rclcpp/rclcpp.hpp"
#include "tf2/LinearMath/Quaternion.hpp"
#include "tf2_ros/transform_broadcaster.h"
#include "turtlesim/msg/pose.hpp"

namespace ns_sub_broadcaster
{
class sub_broadcaster : public rclcpp::Node
{
private:
  rclcpp::Subscription<turtlesim::msg::Pose>::SharedPtr sub_bc_;
  std::shared_ptr<tf2_ros::TransformBroadcaster> tf2_bc_;
  
  void sub_cb(const turtlesim::msg::Pose& pose)
  {
    geometry_msgs::msg::TransformStamped tf;
    tf.header.stamp = this->get_clock()->now();
    tf.header.frame_id = "world";
    tf.child_frame_id = "turtle1";

    tf.transform.translation.x = pose.x;
    tf.transform.translation.y = pose.y;
    tf.transform.translation.z = 0;
    
    tf2::Quaternion q;
    q.setRPY(0,0,pose.theta);
    tf.transform.rotation.x = q.x();
    tf.transform.rotation.y = q.y();
    tf.transform.rotation.z = q.z();
    tf.transform.rotation.w = q.w();

    tf2_bc_->sendTransform(tf);
  }

public:
  sub_broadcaster(/* args */);
  ~sub_broadcaster();
};

sub_broadcaster::sub_broadcaster(/* args */) : Node("sub_broadcaster_node")
{
  sub_bc_ = this->create_subscription<turtlesim::msg::Pose>(
      "turtle1/pose", 10, std::bind(&sub_broadcaster::sub_cb, this, std::placeholders::_1));

    tf2_bc_ = std::make_shared<tf2_ros::TransformBroadcaster>(*this);
}

sub_broadcaster::~sub_broadcaster()
{
}

}  // namespace ns_sub_broadcaster

int main(int argc, char const* argv[])
{
  rclcpp::init(argc, argv);
  auto node = std::make_shared<ns_sub_broadcaster::sub_broadcaster>();
  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}
